#!/usr/bin/env python3

# 导入必要的库
import RPi.GPIO as GPIO  # 用于控制树莓派GPIO
import time              # 用于时间相关操作
import matplotlib.pyplot as plt  # 绘制曲线导入pylot库

class SpeedMeter(object):
    def __init__(self, LS, RS, window_s=1.0):
        # 初始化左右轮的速度记录列表
        self.__r_rec = []  # 右轮传感器触发时间记录
        self.__l_rec = []  # 左轮传感器触发时间记录
        # 配置传感器引脚（BCM编号）
        self.__ls = LS    # 左轮传感器引脚号
        self.__rs = RS    # 右轮传感器引脚号
        # 设置时间窗口长度
        self.__window = window_s  # 用于计算瞬时速度的时间窗口

    def start(self):
        # 注册传感器中断检测（上升沿触发）
        GPIO.add_event_detect(self.__ls, GPIO.RISING, callback=self.__cb_handler)  # 左轮传感器中断
        GPIO.add_event_detect(self.__rs, GPIO.RISING, callback=self.__cb_handler)  # 右轮传感器中断
        time.sleep(self.__window)  # 等待初始数据采集

    def stop(self):
        # 移除传感器中断检测
        GPIO.remove_event_detect(self.__ls)
        GPIO.remove_event_detect(self.__rs)

    def __cb_handler(self, channel):
        # 传感器中断回调函数（通道触发时自动调用）
        t = time.time()  # 获取时间戳
        if channel == self.__ls:
            # 左轮传感器触发处理
            self.__l_rec.append(t)  # 记录触发时间
            # 清理过期数据（超出时间窗口的记录）
            while (t - self.__l_rec[0] > self.__window):
                self.__l_rec.pop(0)  # 移除最旧记录
                if len(self.__l_rec) == 0:  # 防止列表为空时出错
                    break
        else:
            # 右轮传感器触发处理
            self.__r_rec.append(t) # 记录触发时间
            # 清理过期数据（超出时间窗口的记录）
            while (t - self.__r_rec[0] > self.__window):
                self.__r_rec.pop(0) # 移除最旧记录
                if len(self.__r_rec) == 0: # 防止列表为空时出错
                    break

    def get_l_speed(self):
        # 计算左轮转速（转/秒）585为编码器每转脉冲数
        return len(self.__l_rec) / 585.0 / self.__window
    
    def get_r_speed(self):
        # 计算右轮转速（转/秒）
        return len(self.__r_rec) / 585.0 / self.__window

# 计算偏差的公式
#   e(k) = target - feedback(k) 
# 获得位置式PID
#   U(k) = Kp*e(k) + Ki*Integral(0, k, e(k)) + kd*(e(k)-e(k-1))
# 获得增量式PID
#   delta_U(k) = U(k) - U(k-1)
#              = Kp*(e(k)-e(k-1)) + Ki*e(k) + Kd*(e(k)-2*e(k-1)+e(k-2))

# PID控制器实现（增量式算法）
class PID(object):
    def __init__(self, pid_params, target, init_u, u_range):
        # 初始化PID参数 [Kp, Ki, Kd]
        self.__Kp = pid_params[0]  # 比例系数
        self.__Ki = pid_params[1]  # 积分系数
        self.__Kd = pid_params[2]  # 微分系数
        self.__target = target    # 目标设定值
        self.__u = init_u         # 控制量初始值
        self.__u_min = u_range[0] # 控制量下限
        self.__u_max = u_range[1] # 控制量上限

        # 初始化误差记录（当前、前一次、前两次误差）
        self.__e = self.__target   # e(k)
        self.__e_1 = self.__target # e(k-1)
        self.__e_2 = self.__target # e(k-2)

    def update(self, feedback):
        # 更新PID状态并计算新的控制量
        # 误差历史记录更新
        self.__e_2 = self.__e_1
        self.__e_1 = self.__e
        self.__e = self.__target - feedback  # 计算当前误差
        
        # 增量式PID计算公式
        du = (self.__Kp * (self.__e - self.__e_1) +   # 比例项
              self.__Ki * self.__e +                  # 积分项
              self.__Kd * (self.__e - 2*self.__e_1 + self.__e_2))  # 微分项
        
        # 更新控制量并限制在允许范围内
        self.__u += du
        self.__u = max(self.__u_min, min(self.__u, self.__u_max))
        return self.__u

# 硬件配置参数
# GPIO引脚定义（BCM编号）
EA, I2, I1, EB, I4, I3, LS, RS = (13, 19, 26, 16, 20, 21, 6, 12)
# 电机控制参数
FREQUENCY = 50          # PWM频率（Hz）
EXPECTED_SPEED = 3.5    # 目标转速（转/秒）
INIT_DUTY_L = 37        # 左电机初始占空比
PID_PARAMS_L = (4, 2.8, 1.3)  # 左轮PID参数 [Kp, Ki, Kd]
INIT_DUTY_R = 33        # 右电机初始占空比
PID_PARAMS_R = (4, 3.2, 1.2)  # 右轮PID参数 [Kp, Ki, Kd]

# 初始化GPIO
GPIO.setmode(GPIO.BCM)  # 使用BCM编号模式
# 设置引脚模式
GPIO.setup([EA, I2, I1, EB, I4, I3], GPIO.OUT)  # 电机控制引脚设为输出
GPIO.setup([LS, RS], GPIO.IN)                  # 传感器引脚设为输入
# 初始化电机方向控制信号（I1低电平，I2高电平对应正转）
GPIO.output([EA, I1, EB, I4], GPIO.LOW)
GPIO.output([I2, I3], GPIO.HIGH)

# 创建PWM实例
pwm_l = GPIO.PWM(EA, FREQUENCY)  # 左电机PWM
pwm_r = GPIO.PWM(EB, FREQUENCY)  # 右电机PWM
# 初始化测速模块
meter = SpeedMeter(LS, RS, 1.0)  # 时间窗口1秒
meter.start()  # 启动速度测量

# 初始化PID控制器
pid_l = PID(PID_PARAMS_L, EXPECTED_SPEED, INIT_DUTY_L, (0, 100))
pid_r = PID(PID_PARAMS_R, EXPECTED_SPEED, INIT_DUTY_R, (0, 100))

# 数据记录列表
ts = [0.0]       # 时间轴数据
speed_ls = [0.0] # 左轮速度记录
speed_rs = [0.0] # 右轮速度记录

# 主控制循环
t0 = time.time()  # 记录起始时间
pwm_l.start(INIT_DUTY_L)  # 启动左电机PWM
pwm_r.start(INIT_DUTY_R)  # 启动右电机PWM
try:
    while True:
        # 获取并调节左轮速度
        speed_l = meter.get_l_speed()
        pwm_l.ChangeDutyCycle(pid_l.update(speed_l))
        
        # 获取并调节右轮速度
        speed_r = meter.get_r_speed()
        pwm_r.ChangeDutyCycle(pid_r.update(speed_r))
        
        # 记录当前状态
        ts.append(time.time() - t0)  # 记录相对时间
        speed_ls.append(speed_l)     # 记录左轮速度
        speed_rs.append(speed_r)     # 记录右轮速度
        
        time.sleep(0.49)  # 控制循环周期约0.5秒（0.49+处理时间）
except KeyboardInterrupt:    
    # 中断处理：清理资源
    pwm_l.stop()  # 停止PWM输出
    pwm_r.stop()
    meter.stop()  # 停止测速
    GPIO.cleanup()  # 复位GPIO状态

# 绘制速度曲线
plt.plot(ts, speed_ls, '-o')
plt.plot(ts, speed_rs, '-*')
plt.plot(ts, list([EXPECTED_SPEED for t in ts]), '-') # 绘制目标速度线
plt.show()